﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Threading.Tasks;

namespace CarSimulate
{
    internal class CarDistance
    {
        /// <summary>
        /// 计算两个线段之间的最短距离
        /// </summary>
        /// <param name="seg1Start">线段1起点</param>
        /// <param name="seg1End">线段1终点</param>
        /// <param name="seg2Start">线段2起点</param>
        /// <param name="seg2End">线段2终点</param>
        /// <returns>两条线段之间的最短距离</returns>
        public static double DistanceBetweenSegments(PointF seg1Start, PointF seg1End,
                                                    PointF seg2Start, PointF seg2End)
        {
            // 检查两条线段是否相交
            if (SegmentsIntersect(seg1Start, seg1End, seg2Start, seg2End))
                return 0.0;

            // 计算四种端点组合的最小距离
            double minDistance = double.MaxValue;
            minDistance = Math.Min(minDistance, DistanceToLineSegment(seg1Start, seg2Start, seg2End));
            minDistance = Math.Min(minDistance, DistanceToLineSegment(seg1End, seg2Start, seg2End));
            minDistance = Math.Min(minDistance, DistanceToLineSegment(seg2Start, seg1Start, seg1End));
            minDistance = Math.Min(minDistance, DistanceToLineSegment(seg2End, seg1Start, seg1End));

            return minDistance;
        }
        internal static double DistanceBetweenSegments(CarInLineStruct carPos1, CarInLineStruct carPos2)
        {
            return DistanceBetweenSegments(carPos1.LastPoint, carPos1.CurrentPoint, carPos2.LastPoint, carPos2.CurrentPoint);
        }

        /// <summary>
        /// 判断两条线段是否相交
        /// </summary>
        private static bool SegmentsIntersect(PointF a1, PointF a2, PointF b1, PointF b2)
        {
            // 快速排斥实验
            if (Math.Max(a1.X, a2.X) < Math.Min(b1.X, b2.X) ||
                Math.Max(b1.X, b2.X) < Math.Min(a1.X, a2.X) ||
                Math.Max(a1.Y, a2.Y) < Math.Min(b1.Y, b2.Y) ||
                Math.Max(b1.Y, b2.Y) < Math.Min(a1.Y, a2.Y))
                return false;

            // 跨立实验
            double c1 = CrossProduct(a1, a2, b1);
            double c2 = CrossProduct(a1, a2, b2);
            double c3 = CrossProduct(b1, b2, a1);
            double c4 = CrossProduct(b1, b2, a2);

            // 跨立实验判断相交
            if (c1 * c2 < 0 && c3 * c4 < 0)
                return true;

            // 处理共线情况
            if (c1 == 0 && IsOnSegment(a1, a2, b1)) return true;
            if (c2 == 0 && IsOnSegment(a1, a2, b2)) return true;
            if (c3 == 0 && IsOnSegment(b1, b2, a1)) return true;
            if (c4 == 0 && IsOnSegment(b1, b2, a2)) return true;

            return false;
        }

        /// <summary>
        /// 计算叉积
        /// </summary>
        private static double CrossProduct(PointF a, PointF b, PointF c)
        {
            return (b.X - a.X) * (c.Y - a.Y) - (b.Y - a.Y) * (c.X - a.X);
        }

        /// <summary>
        /// 判断点c是否在线段ab上
        /// </summary>
        private static bool IsOnSegment(PointF a, PointF b, PointF c)
        {
            return Math.Min(a.X, b.X) <= c.X && c.X <= Math.Max(a.X, b.X) &&
                   Math.Min(a.Y, b.Y) <= c.Y && c.Y <= Math.Max(a.Y, b.Y);
        }

        /// <summary>
        /// 计算点到线段的距离（之前实现的函数）
        /// </summary>
        private static double DistanceToLineSegment(PointF point, PointF lineStart, PointF lineEnd)
        {
            double dx = lineEnd.X - lineStart.X;
            double dy = lineEnd.Y - lineStart.Y;
            double lineLengthSquared = dx * dx + dy * dy;

            if (lineLengthSquared == 0)
                return DistanceBetweenPoints(point, lineStart);

            double t = ((point.X - lineStart.X) * dx +
                        (point.Y - lineStart.Y) * dy) / lineLengthSquared;

            t = Math.Max(0, Math.Min(1, t));

            double projectionX = lineStart.X + t * dx;
            double projectionY = lineStart.Y + t * dy;

            return DistanceBetweenPoints(point, new Point((int)projectionX, (int)projectionY));
        }

        #region 2-1 点到线段的距离
        /// <summary>
        /// 计算两点之间的距离
        /// </summary>
        private static double DistanceBetweenPoints(PointF a, PointF b)
        {
            double dx = a.X - b.X;
            double dy = a.Y - b.Y;
            return Math.Sqrt(dx * dx + dy * dy);
        }

        public static double DistanceToLineSegmentOptimized(PointF point, PointF lineStart, PointF lineEnd)
        {
            double dx = lineEnd.X - lineStart.X;
            double dy = lineEnd.Y - lineStart.Y;
            double lineLengthSquared = dx * dx + dy * dy;

            if (lineLengthSquared == 0)
                return DistanceBetweenPointsOptimized(point, lineStart);

            double t = ((point.X - lineStart.X) * dx +
                        (point.Y - lineStart.Y) * dy) / lineLengthSquared;

            t = Math.Max(0, Math.Min(1, t));

            double projectionX = lineStart.X + t * dx;
            double projectionY = lineStart.Y + t * dy;

            double deltaX = point.X - projectionX;
            double deltaY = point.Y - projectionY;
            return Math.Sqrt(deltaX * deltaX + deltaY * deltaY);
        }

        private static double DistanceBetweenPointsOptimized(PointF a, PointF b)
        {
            double dx = a.X - b.X;
            double dy = a.Y - b.Y;
            return Math.Sqrt(dx * dx + dy * dy);
        } 
        #endregion
    }
}
